{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Drake的术语和符号\n",
    "\n",
    "【描述】\n",
    "\n",
    "【内容】\n",
    "- 准确性、公差、和精度\n",
    "\n",
    "多数数值方法都是近似的，用户可以控制代码运行速度和计算质量之间的折中。\n",
    "\n",
    "- 多体平台的术语和符号\n",
    "\n",
    "将多体机构的数学描述转换为正确的代码是一个困难的过程，需要仔细的训练以便得到正确的程序。\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1、准确性、公差、和精度"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2、多体平台的术语和符号\n",
    "\n",
    "在Drake中我们使用特别设计的术语、符号和抽象，以降低错误的可能，主要原因在于：\n",
    "- 好的抽象会提高代码质量\n",
    "- 分开处理移动和旋转时不能很好的推导空间算法\n",
    "- 严格的符号表示对避免坐标系错误非常重要\n",
    "- 代码中的算法应该能与产生他们的方程一一对应\n",
    "- 我们需要一种公认的和清晰的符号，这样可以利用对程序员来说很好用的模式匹配技巧，当程序中存在错误时可以有效的找出来\n",
    "\n",
    "本章中我们给出代码文档中出现的各种术语、符号和抽象。\n",
    "这些知识对于正在使用Drake多体平台代码的程序员非常有用。\n",
    "虽然其中一些解释是针对特定开发者的，但是大多数信息对使用Drake应用程序接口的用户来说都是有用的，因此也包含在外部文档里。\n",
    "（外部文档指的应该就是本文档，而由于文档是从代码注释生成的，因此包含了一些特定的注释。作者的意思应该是这些注释对于Drake的开发者来说意义更大，但也有参考价值）\n",
    "\n",
    "只要可能，我们更愿意发布文献，以补充我们的代码文档。\n",
    "文献可以提供清楚、一致和无歧义的公式和算法，不过需要对公式进行排版。\n",
    "我们需要将公式转换为代码，。。\n",
    "Drake中的符号设计思想就是要利于排版与代码之间的比对，以便校正实现时的错误。\n",
    "\n",
    "另外，我们也需要开发者提供好的本地文档，这些文档对应数学算法，而且要以Doxygen注释的形式提供。\n",
    "这些文档在Doxygen格式化后非常漂亮。\n",
    "但是，对于高效的开发，我们需要这些文档易于理解和修改，因为开发者要直接在源代码上开展工作。\n",
    "让格式化后的文档变得漂亮，通常会提高文档撰写的难度。\n",
    "\n",
    "不过，对术语做一些妥协还是可以达到上面的目标的。\n",
    "如果我们使用排版数学符号时，就避免了转换成代码的困难。\n",
    "例如，我们不使用手写字体，也不会将箭头放在向量上面。\n",
    "同时，由于多体动力学中几乎所有的量都是向量或矩阵，我们不遵循通常的惯例，不使用粗体。原因是：（1）所有的量都是粗体也不会让文档更清晰；（2）不能在代码或注释中使用粗体。\n",
    "我们确实使用有限的上标和下标，但是提供了与代码中各量的对应。\n",
    "我们使用希腊符号，但是在代码中提供了应为对应。\n",
    "在可能的情况下，我们首选UTF-8编码，而不是LaTeX。\n",
    "\n",
    "【内容】\n",
    "\n",
    "- 符号基础\n",
    "\n",
    "表示位置、姿态、惯量和空间速度\n",
    "\n",
    "- 坐标系和刚体\n",
    "\n",
    "多体动力学中最基本的对象就是坐标系\n",
    "\n",
    "- 多体平台中的量\n",
    "\n",
    "对应多体动力学的各个量\n",
    "\n",
    "- 多体平台中量的微分\n",
    "\n",
    "标量x关于时间的一阶微分表示为xdot或xDt，二阶微分表示为xddot或xDDt\n",
    "\n",
    "- 空间代数\n",
    "\n",
    "包括平移和旋转在内的多体动力学\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.1 符号基础\n",
    "\n",
    "若Q表示任意的物理量。\n",
    "为了向量计算，我们必须选择一组基来表示各个独立的数值元素。\n",
    "“基”就是一组独立的方向向量，例如三维空间中的xyz坐标轴。\n",
    "为了表示向量在这组基中的坐标值，需要将向量与每个坐标轴执行点积计算。\n",
    "\n",
    "所有合理的基都能在计算中使用；基的选择不改变物理量的意义。\n",
    "不过，由于笛卡尔基总是坐标系的轴，我们通常使用它。\n",
    "\n",
    "假设提供基的坐标系是F。\n",
    "排版方程中我们使用$[Q]_F$来表示。\n",
    "而在代码或注释中，我们将其转换为组合字母，比如Q_F。\n",
    "在本文档中，我们通常使用默认字体。\n",
    "\n",
    "一般的物理量包括下面的部分：\n",
    "- 表示物理量类型的符号，如速度v或惯量I\n",
    "- 参考符号\n",
    "- 目标符号\n",
    "- 从一组量里选择特定量的索引\n",
    "\n",
    "与质量相关的量可能还包括额外的“关于”点，这将在后面讨论。\n",
    "\n",
    "【注意】物理量是坐标系无关的，使用坐标系相关的数值表示是为了计算。\n",
    "\n",
    "有些物理量可能不包含所有的部分。当包括所有的部分时，看起来就是下面的样子：\n",
    "\n",
    "$$\n",
    "^RQ_i^T\n",
    "$$\n",
    "\n",
    "其中，R是参考，T是目标，i是索引\n",
    "对应的字母组合符号，将是Qi_RT。\n",
    "\n",
    "如果需要给出坐标系，则物理量表示为：\n",
    "\n",
    "$$\n",
    "[^RQ_i^T]_F\n",
    "$$\n",
    "\n",
    "代码中则是Qi_RT_F。\n",
    "考虑一个真实的例子，物体B（目标）相对于物体A（参考）的角速度描述为$^A\\omega^B$，对应的字母组合是w_AB。如果该向量是世界坐标系下的表示，需要写$[^A\\omega^B]_W$，对应的字母组合是w_AB_W。\n",
    "需要指出的是，如果参考符号具有明确的标架，对应的基就是默认的坐标系。\n",
    "例如，w_AB可以代替w_AB_A\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.2 标架和物体\n",
    "\n",
    "除非特别说明，所有的标架都是右手笛卡尔标架。\n",
    "\n",
    "世界标架W是独特的惯性标架，有时称为地面G或牛顿标架N。\n",
    "Drake支持模型标架符号，是在世界标架中固定的，因此也是惯性标架。\n",
    "这样，整个仿真可以建立在多个独立的模型上，每个模型可以相对于其自身的模型标架来描述。\n",
    "这对应sdf中的\\<model\\>标签。\n",
    "\n",
    "为了简化符号，我们允许将标架指定为一个无歧义的上下文，其中只包含一个点和一组基。\n",
    "\n",
    "一个物体就是一个标架，因此我们对物体和它的标架都使用相同的符号。\n",
    "例如，物体B有相关的物体标架B。\n",
    "当我们说一个物体的“位置”和“方向”时，意思就是物体标架原点Bo的位置和物体标架B的方向。\n",
    "物体特性，例如惯量和几何形状，都是相对于物体标架给出的。\n",
    "物体的质心表示为$B_{cm}$，组合符号时Bcm，它在物体上的位置由Bcm相对于Bo的位置表示。\n",
    "对于刚体来说，固连在其上的所有标架相对于物体标架都是固定的。\n",
    "对于柔性体来说，形变也是相对于物体标架来测量的。\n",
    "\n",
    "当用户在初始阶段指定一个物体，例如sdf文件或urdf文件中的\\<link\\>标签，有一个区别于物体标架B的连杆标架L，Drake用L进行内部计算。\n",
    "不过，标架L和标架B之间通过一个仿真中不变的定常变换即可计算。\n",
    "用户提供的信息，包括质量、图形和碰撞几何都是相对于标架L给出的。\n",
    "Drake在内部会将这些信息变换到标架B下。\n",
    "\n",
    "#### 偏移标架的符号\n",
    "\n",
    "给定一个标架F，我们通常还需要另一个与其固连的标架。\n",
    "这个标架的坐标轴方向与F一致，但是原点移动到另外的地方。\n",
    "我们称其为偏移标架。\n",
    "在数学符号中我们使用$F_R$来表示。\n",
    "在代码中我们没有下标，因此使用小写符号表示偏移点的位置，表示为Fr。\n",
    "\n",
    "我们把标架P在世界坐标系中的空间速度表示为V_WP($^WV^P$)，而将标架B在标架P中的空间速度表示为V_PB。\n",
    "如果想得到V_WB，也就是标架B在世界坐标系中的空间速度。\n",
    "为了达到这一点，我们需要将V_WP偏移成V_WPb。\n",
    "注意到Pb是一个标架，其方向与P一致，但原点在Bo处。\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.3 多体动力学量\n",
    "\n",
    "多体动力学中的量有着不同的类型。\n",
    "\n",
    "例如，旋转矩阵表示为R，位置向量表示为p。\n",
    "同时对已有的量求微分可以得到新的量。\n",
    "\n",
    "大多数量都有一个参考和一个目标，二者可能是标架、基或者点。\n",
    "在计算中，向量定义在特定的基上。\n",
    "例如，点P在参考标架F中的速度是一个向量，目标为点P，参考为标架F。\n",
    "排版中该符号写为$^Fv^P$。\n",
    "计算中，该向量是在特定基下的描述。通常，表示坐标系与参考坐标系相同。在前面的例子中，表示坐标系也是标架F。\n",
    "不过，还可以有不同的表示，比如在世界坐标系中的表示，记为$[^Fv^P]_W$。\n",
    "\n",
    "考虑另外一个例子，点Bcm相对于点Bo的位置向量，在B中的表示。\n",
    "在排版时，记为$[^{Bo}p^{Bcm}]_B$，可简写成$^{B}p^{Bcm}$。\n",
    "\n",
    "#### 多体动力学量的例子\n",
    "\n",
    "。。。\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.4 多体动力学量的时间微分\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.5 空间代数\n",
    "\n",
    "【描述】\n",
    "\n",
    "相比独立的处理，使用群理论表示空间代数更加有效。\n",
    "\n",
    "这里我们给出Drake中使用的多体动力学的描述方法，以及如何在代码中表示物理量（通常是Eigen对象）。\n",
    "\n",
    "【模块】\n",
    "\n",
    "- 空间位形和变换\n",
    "\n",
    "空间位形给出了标架B在标架A中的位置和方向\n",
    "\n",
    "- 空间向量\n",
    "\n",
    "空间向量是一个由两个三维向量构成的6元数\n",
    "\n",
    "- 空间质量矩阵（空间惯量）\n",
    "\n",
    "空间质量矩阵M是一个6x6矩阵，表示质量、质心位置和惯量。该矩阵是正定的，由四个3x3子矩阵构成。\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 2.5.1 空间位形和变换\n",
    "\n",
    "空间变换，或变换，是位形的复合形式。\n",
    "它是一个易于从位形信息中得到的线性操作。\n",
    "\n",
    "- 位置\n",
    "\n",
    "- 方向\n",
    "\n",
    "向量r从F中的表示变换到G中的表示\n",
    "\n",
    "r_G = R_GF * r_F\n",
    "\n",
    "而且旋转矩阵是正交阵，因此其逆就是它的转置\n",
    "\n",
    "r_F = R_FG * r_G  or  r_F = R_GF.transpose() * r_G\n",
    "\n",
    "旋转操作很容易复合\n",
    "\n",
    "R_WC = R_WA * R_AB * R_BC\n",
    "\n",
    "我们一般使用四元数作为一种更紧凑的旋转矩阵表示，在程序中使用Eigen::Quaternion类来表示它们。\n",
    "概念上，q_GF与R_GF具有相同的含义。\n",
    "\n",
    "- 刚体变换\n",
    "\n",
    "（1）表示的变换\n",
    "\n",
    "（2）变换的复合\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 2.5.2 空间向量\n",
    "\n",
    "Drake中的空间向量是两个三维向量，而不是一个六维向量。\n",
    "不过，我们仍然可以将其作为6元列向量进行操作，这样就不用区分平移操作和旋转操作。\n",
    "前三个量是旋转量，后三个量是平移量。\n",
    "\n",
    "\\\n",
    "      Spatial: Velocity  Acceleration   Force\n",
    "                 ---         ---         ---    Eigen access\n",
    "              0 |   |       |   |       |   |\n",
    "    rotation  1 | ω |       | α |       | τ |   .head<3>()\n",
    "              2 |   |       |   |       |   |\n",
    "                 ---         ---         ---\n",
    "              3 |   |       |   |       |   |\n",
    " translation  4 | v |       | a |       | f |   .tail<3>()\n",
    "              5 |   |       |   |       |   |\n",
    "                 ---         ---         ---\n",
    "       Symbol:    V           A           F\n",
    "\n",
    "\\\n",
    "\n",
    "【注意】空间速度有时被称为旋量；而空间力有时被称为力偶。为了避免混淆，我们不使用这些术语。\n",
    "\n",
    "空间向量的旋转部分是应用在刚体或标架的整体上的，而平移部分则施加在固定在相同刚体或标架的某一点上。\n",
    "在计算中，两个子向量必须在同一个标架中表示，可以是体坐标系或其他指定的坐标系。\n",
    "因此，空间向量的无歧义符号必须包括一个点和表示标架。\n",
    "运动量也必须给出参考系。\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 2.5.3 空间质量矩阵\n",
    "\n",
    "空间质量矩阵M是一个6x6矩阵，表示质量、质心位置和惯量。该矩阵是正定的，由四个3x3子矩阵构成。\n",
    "\n",
    "\\\n",
    "            Spatial mass matrix\n",
    "           ---------- ----------\n",
    "        0 |          |          |\n",
    "        1 |  I(=mG)  |   m c×   |\n",
    "        2 |          |          |\n",
    "           ---------- ----------\n",
    "        3 |          |          |\n",
    "        4 |  -m c×   |    mE    |\n",
    "        5 |          |          |\n",
    "           ---------- ----------\n",
    "               Symbol: M\n",
    "\\\n",
    "\n",
    "上面的符号表示一个物体B的空间惯量M，其中的量是相对于点P获得的。\n",
    "\n",
    "实际上，上面具有36个元素的矩阵只有10个独立元素，因此我们可以有更紧凑的表示。\n",
    "\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
